import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener, Buffer
from tf_transformations import euler_from_quaternion  # 四元数到欧拉角度的转换

class TFListener(Node):
    def __init__(self):
        super().__init__('tf_listener')
        self.tf_buffer = Buffer() # 创建缓冲器
        self.tf_listener = TransformListener(self.tf_buffer, self) # 创建监听器
        self.timer = self.create_timer(1.0, self.listen_to_tf) # 创建定时器，每秒执行一次回调函数

    def listen_to_tf(self):
        try:
            result= self.tf_buffer.lookup_transform('base_link','bottle_link' ,rclpy.time.Time(seconds=0.0),rclpy.time.Duration(seconds=1.0))
            transform = result.transform
            self.get_logger().info(f"平移{transform.translation}")
            self.get_logger().info(f"旋转{transform.rotation}")
            rotation_euler = euler_from_quaternion([transform.rotation.x,transform.rotation.y,transform.rotation.z,transform.rotation.w])
            self.get_logger().info(f"旋转RPY{rotation_euler}")

        except Exception as e:
            self.get_logger().error(f'不能获取坐标关系，原因： {e}')


def main(args=None):
    rclpy.init(args=args)
    node = TFListener()
    rclpy.spin(node)
    rclpy.shutdown()
        
